#include <string.h>
#include <unistd.h>
#include <fcntl.h>
#include <termios.h>
#include <errno.h>
#include <stdint.h>
#include <pthread.h>
#include <thread>
#include <stdio.h>
#include <stdlib.h>
#include <iostream>
#include <mutex>
#include <angles/angles.h>
#include <rclcpp/rclcpp.hpp>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
// 包含消息类型的头文件
#include "auto_aim_interfaces/msg/tracker_info.hpp"
#include "auto_aim_interfaces/msg/target.hpp"
#include "auto_aim_interfaces/msg/armors.hpp"

#include "demo_cpp_tf/simple_kalman_filter.hpp"
#include <Eigen/Dense>
#include <deque>
#include <queue>
#include <tcl8.6/tcl-private/compat/unistd.h>

class SerialNode : public rclcpp::Node
{
    public:
        SerialNode() : Node("serial_node"), send_yaw(0.0), send_pitch(0.0), distance(0.0),
                  serial_fd(-1), fd1(-1), fd2(-1), fd3(-1), fire(0),send_state(1), 
                  image_width(640.0), image_height(480.0),focal_length(500.0), 
                  is_kf_initialized(false), last_armor_time_(this->now())
        {
            // 要执行的命令
            // const char* command = "sudo chmod 666 /dev/ttyUSB0";
            
            // 打开串口设备
            fd1 = open("/dev/ttyUSB0", O_RDWR | O_NOCTTY | O_NDELAY);
            fd2 = open("/dev/ttyUSB1", O_RDWR | O_NOCTTY | O_NDELAY);
            fd3 = open("/dev/ttyUSB2", O_RDWR | O_NOCTTY | O_NDELAY);

            if(-1 != fd1)
            {
                RCLCPP_INFO(get_logger(), "ttyUSB0打开成功");
                serial_fd = fd1;
            } 
            else if(-1 != fd2)
            {
                RCLCPP_INFO(get_logger(), "ttyUSB1打开成功");
                serial_fd = fd2;
            }
            else if(-1 != fd3)
            {
                RCLCPP_INFO(get_logger(), "ttyUSB2打开成功");
                serial_fd = fd3;
            }
            else
                RCLCPP_ERROR(this->get_logger(), "Failed to open serial port: %s", strerror(errno));
                
            // 配置串口参数
            struct termios options;
            tcgetattr(serial_fd, &options);
            cfsetispeed(&options, B115200);  // 设置波特率为 115200
            cfsetospeed(&options, B115200);
            options.c_cflag |= (CLOCAL | CREAD);
            options.c_cflag &= ~PARENB;  // 无校验位
            options.c_cflag &= ~CSTOPB;  // 1 位停止位
            options.c_cflag &= ~CSIZE;
            options.c_cflag |= CS8;      // 8 位数据位
            tcsetattr(serial_fd, TCSANOW, &options);    
            
            // 创建 QoS 对象
            auto qos = rclcpp::SensorDataQoS();

            // 创建 /detector/armors 话题的订阅者
            armors_sub_ = this->create_subscription<auto_aim_interfaces::msg::Armors>(
                "/detector/armors", qos, std::bind(&SerialNode::armorsCallback, this, std::placeholders::_1));

            // 从参数服务器获取图像宽度、高度和相机焦距
            image_width = this->declare_parameter("image_width", 640.0);
            image_height = this->declare_parameter("image_height", 480.0);
            focal_length = this->declare_parameter("focal_length", 500.0);

            // 初始化卡尔曼滤波器（7维状态：x,y,z, vx,vy,vz）
            kf_ = std::make_unique<demo_cpp_tf::SimpleKalmanFilter>(7, 3);
            kf_->enableNonlinearMode(true);

            // 初始化 last_aim_time_ 为当前时间
            last_aim_time_ = this->now();

            // 启动接收线程
            receive_thread_ = std::thread(&SerialNode::receiveData, this);

            // 启动发送线程
            send_thread_ = std::thread(&SerialNode::sendDataThread, this);

            // 添加定时器（每秒检查一次）
            check_timer_ = this->create_wall_timer(
                std::chrono::milliseconds(500),
                std::bind(&SerialNode::checkArmorPresence, this));
        }

        ~SerialNode()
        {
            // 关闭串口设备
            if (serial_fd != -1)
            {
                std::lock_guard<std::mutex> lock(serial_mutex);
                close(serial_fd);
                serial_fd = -1;
            }

            // 等待接受线程结束
            if(receive_thread_.joinable())
            {
                receive_thread_.join();
            }

            // 等待发送线程结束
            if (send_thread_.joinable())
            {
                send_thread_.join();
            }
        }
    private:
        // /detector/armors 话题的回调函数
        void armorsCallback(const auto_aim_interfaces::msg::Armors::SharedPtr msg)
        {
            // 有装甲板时的处理逻辑
            for (const auto& armor : msg->armors)
            {
                send_state = 2;
                if (!msg->armors.empty()) 
                {
                    // 获取第一个检测到的装甲板
                    auto armor = msg->armors[0];
                    
                    // 获取当前时间
                    auto current_time = this->now();
                    static auto last_time = current_time;
                    double dt = (current_time - last_time).seconds();
                    last_time = current_time;

                    distance = std::sqrt(armor.pose.position.x * armor.pose.position.x + 
                        armor.pose.position.y * armor.pose.position.y + armor.pose.position.z * armor.pose.position.z);

                    // 坐标系转换：将检测到的装甲板坐标转换到世界坐标系
                    Eigen::Vector3d z(
                        armor.pose.position.x,
                        armor.pose.position.y,
                        armor.pose.position.z
                    );

                    if (!is_kf_initialized) 
                    {
                        kf_.reset(); // 显式释放旧实例
                        // 扩展状态变量：增加角速度维度 [x, y, z, vx, vy, vz, ω]
                        kf_ = std::make_unique<demo_cpp_tf::SimpleKalmanFilter>(7, 3);
                        Eigen::VectorXd x0(7);
                        x0 << z[0], z[1], z[2], 0, 0, 0, 0; // 初始角速度设为0
                        kf_->init(x0);
                        is_kf_initialized = true;
                    } 
                    else 
                    {
                        /******************** 使用匀速圆周运动模型 ********************/
                        // 状态转移矩阵（考虑角速度ω）
                        Eigen::MatrixXd F(7,7);
                        double omega = kf_->getState()[6];
                        F << 1, 0, 0, dt, 0, 0, -z[1]*dt,  // x方向包含旋转分量
                            0, 1, 0, 0, dt, 0, z[0]*dt,   // y方向包含旋转分量
                            0, 0, 1, 0, 0, dt, 0,         // z方向
                            0, 0, 0, 1, 0, 0, -omega*dt,  // vx
                            0, 0, 0, 0, 1, 0, omega*dt,   // vy
                            0, 0, 0, 0, 0, 1, 0,          // vz
                            0, 0, 0, 0, 0, 0, 1;          // ω
                        
                        // 过程噪声调整（增大旋转方向的噪声）
                        Eigen::MatrixXd Q(7,7);
                        Q << 0.1, 0,   0,   0,   0,   0,   0,
                            0,   0.1, 0,   0,   0,   0,   0,
                            0,   0,   0.1, 0,   0,   0,   0,
                            0,   0,   0,   0.5, 0,   0,   0,
                            0,   0,   0,   0,   0.5, 0,   0,
                            0,   0,   0,   0,   0,   0.1, 0,
                            0,   0,   0,   0,   0,   0,   0.2;  // 角速度维度的噪声较大
                        
                        kf_->setTransitionMatrix(F);
                        kf_->setProcessNoiseCov(Q);

                        /******************** 角度补偿预测 ********************/
                        // 预测和更新步骤
                        kf_->predict(dt);

                        // 使用非线性观测模型（扩展卡尔曼滤波）
                        Eigen::Vector3d h = kf_->getState().head(3); // 预测位置
                        Eigen::MatrixXd H = Eigen::MatrixXd::Zero(3,7);
                        H.block(0,0,3,3) = Eigen::Matrix3d::Identity();
                        
                        // 更新步骤
                        kf_->update(z, H);

                        /******************** 动态调整预测窗口 ********************/
                        // 根据角速度调整预测时间
                        double effective_dt = dt;
                        if(std::abs(omega) > 0.5) // 检测到高速旋转时
                        { 
                            effective_dt = dt * 1.5; // 增加预测时间窗口
                            RCLCPP_WARN(get_logger(), "High rotation detected! omega=%.2f rad/s", omega);
                        }
                        
                        /******************** 状态提取和角度计算 ********************/
                        Eigen::VectorXd state = kf_->getState();
                        double pred_x = state[0] + state[3]*effective_dt;
                        double pred_y = state[1] + state[4]*effective_dt;
                        double pred_z = state[2] + state[5]*effective_dt;

                        // 考虑旋转补偿
                        double theta = state[6] * effective_dt; // 预测旋转角度
                        Eigen::Vector3d pred_pos = Eigen::Vector3d(pred_x, pred_y, pred_z);
                        pred_pos = Eigen::AngleAxisd(theta, Eigen::Vector3d::UnitZ()) * pred_pos;

                        // 计算最终角度，手动添加弹道补偿
                        double compensated_y = pred_pos[1]; // 调整y坐标
                        double compensated_x = pred_pos[0]; // 调整x坐标
                        if(distance > 0.1 && 1.0 > distance)
                        {
                            compensated_y -= 0.01;
                            // compensated_x += 0.005;
                        }
                        else if(distance >= 1.0 && 1.5 > distance)
                        {
                            compensated_y -= 0.07;
                            // compensated_y -= distance*0.07;
                        }
                        else if(distance >= 1.5 && 2.0 > distance)
                        {
                            compensated_y -= 0.12;
                            compensated_x -= 0.015;
                        }
                        else if(distance >= 2.0 && 2.5 > distance)
                        {
                            compensated_y -= 0.135;
                            compensated_x -= 0.035;
                        }
                        else if(distance >= 2.5 && 3.0 > distance)
                        {
                            compensated_y -= 0.20;
                            compensated_x -= 0.05;
                        }
                        else if(distance >= 3.0)    // 没测
                        {
                            compensated_y -= distance*0.1;
                            compensated_x -= distance*0.01;
                        }
                            
                        send_yaw = std::atan2(compensated_x, pred_pos[2]);
                        send_pitch = std::atan2(compensated_y, pred_pos[2]);

                        // 角度平滑处理
                        static double last_yaw = 0, last_pitch = 0;
                        send_yaw = 0.7*send_yaw + 0.3*last_yaw;
                        send_pitch = 0.7*send_pitch + 0.3*last_pitch;
                        last_yaw = send_yaw;
                        last_pitch = send_pitch;
                        
                        // 计算当前角度误差
                        double yaw_error = std::abs(send_yaw);
                        double pitch_error = std::abs(send_pitch);
                        // 维护滑动窗口（保留最近STABLE_FRAMES次误差）
                        if(angle_errors_.size() >= STABLE_FRAMES) 
                        {
                            angle_errors_.pop_front();
                        }
                        angle_errors_.emplace_back(yaw_error, pitch_error);
                        // 计算平均误差
                        double avg_yaw_error = 0.0, avg_pitch_error = 0.0;
                        for(const auto& err : angle_errors_) 
                        {
                            avg_yaw_error += err.first;
                            avg_pitch_error += err.second;
                        }
                        avg_yaw_error /= angle_errors_.size();
                        avg_pitch_error /= angle_errors_.size();

                        // 判断稳定条件
                        bool angle_stable = (avg_yaw_error < FIRE_ANGLE_THRESHOLD) && 
                                        (avg_pitch_error < FIRE_ANGLE_THRESHOLD);
                        
                        // 判断距离条件
                        bool distance_valid = (distance > MIN_FIRE_DISTANCE) && 
                                            (distance < MAX_FIRE_DISTANCE);
                        std::lock_guard<std::mutex> lock(state_mutex_);
                        RCLCPP_INFO(get_logger(), "distance : %f", distance);
                        // 持续对准时间判断
                        // if(angle_stable && distance_valid)
                        if(distance_valid) 
                        {
                            if(!is_aiming_stable_) 
                            {
                                last_aim_time_ = this->now();
                                is_aiming_stable_ = true;
                            }
                        } 
                        else 
                        {
                            is_aiming_stable_ = false;
                        }
                        // RCLCPP_INFO(get_logger(), "is_aiming_stable_:%d", is_aiming_stable_);
                        // 判断是否满足开火条件（持续稳定超过0.1秒）
                        const auto stable_duration = (this->now() - last_aim_time_).seconds();
                        if(is_aiming_stable_ && stable_duration > 0.1) 
                        {
                            fire = 2;   // 原2
                            send_state = 2;
                            RCLCPP_INFO(get_logger(), "Fire condition met! Stable time: %.2fs", stable_duration);
                        } 
                        else 
                        {
                            fire = 0;
                        }
                        
                        // 调试输出
                        RCLCPP_INFO(get_logger(), "Calculated Angles: yaw = %frad, pitch = %frad", send_yaw, send_pitch);
                        
                        send_queue_.push({send_yaw, send_pitch, distance, fire, send_state});
                    }
                }
                last_armor_time_ = this->now();   
            }
            // checkArmorPresence();
        }

        // 定时器回调函数
        void checkArmorPresence()
        {
            std::lock_guard<std::mutex> lock(time_mutex_);
            auto now = this->now();
            
            // 计算时间差（秒）
            double time_diff = (now - last_armor_time_).seconds();
            
            // 如果超过1秒没有检测到装甲板
            if (time_diff > 0.2) 
            {
                std::lock_guard<std::mutex> lock_state(state_mutex_);
                std::lock_guard<std::mutex> lock_send(send_mutex_);
                
                // 重置所有状态
                send_yaw = 0.0;
                send_pitch = 0.0;
                distance = 0.0;
                fire = 0;
                send_state = 1;
                is_kf_initialized = false;
                is_aiming_stable_ = false;
                
                // 清空队列
                while (!send_queue_.empty()) 
                {
                    send_queue_.pop();
                }
                
                // 压入清零状态
                send_queue_.push({send_yaw, send_pitch, distance, fire, send_state});
                RCLCPP_WARN(get_logger(), "No armor detected for 0.5s, reset states!");
            }
        }

        // 发送数据的线程函数
        void sendDataThread()
        {
            while (rclcpp::ok())
            {
                // 从队列中获取数据
                SendData data;
                {
                    std::lock_guard<std::mutex> lock(send_mutex_);
                    if (!send_queue_.empty())
                    {
                        data = send_queue_.front();
                        send_queue_.pop();
                    }
                }

                // 发送数据
                if (serial_fd != -1)
                {
                    const int frame_length = 1 + 1 + 1 + sizeof(double) + sizeof(double) + sizeof(double) + 1 + 1;
                    uint8_t buffer[frame_length];

                    // 填充数据帧
                    buffer[0] = 0xFF;
                    buffer[1] = (char)frame_length;
                    buffer[2] = data.send_state;
                    buffer[3] = data.fire;
                    memcpy(buffer + 4, &data.send_yaw, sizeof(double));
                    memcpy(buffer + 4 + sizeof(double), &data.send_pitch, sizeof(double));
                    // RCLCPP_INFO(get_logger(), "这个data.send_pitch: %f", data.send_pitch);
                    memcpy(buffer + 4 + sizeof(double) + sizeof(double), &data.distance, sizeof(double));
                    buffer[frame_length - 1] = 0xFE;

                    // 发送数据
                    ssize_t bytes_written = write(serial_fd, buffer, frame_length);
                    if (bytes_written == -1)
                    {
                        RCLCPP_ERROR(this->get_logger(), "Failed to write to serial port: %s", strerror(errno));
                    }
                    else if (bytes_written != frame_length)
                    {
                        RCLCPP_WARN(this->get_logger(), "Incomplete write to serial port");
                    }
                }

                // 适当休眠，避免CPU占用过高
                std::this_thread::sleep_for(std::chrono::milliseconds(10));
            }
        }

        // 接收数据的线程函数
        void receiveData()
        {
            const uint8_t FRAME_HEADER = 0xFF;  // 帧头
            const uint8_t FRAME_FOOTER = 0xFE;  // 帧尾
            const size_t MAX_FRAME_SIZE = 128;  // 最大帧长度

            uint8_t read_buffer[MAX_FRAME_SIZE];
            size_t buffer_index = 0;    // 当前缓冲区索引
            bool frame_started = false; // 是否已经开始接受一帧数据
            size_t read_frame_length = 0;    // 当前的帧长度

            while (rclcpp::ok())
            {
                std::lock_guard<std::mutex> lock(serial_mutex);
                if (serial_fd != -1)
                {
                    uint8_t byte;
                    ssize_t bytes_read = read(serial_fd, &byte, 1); // 每次读取一个字节
                    if(bytes_read == 1)
                    {
                        if(!frame_started)
                        {
                            // 寻找帧头
                            if(byte == FRAME_HEADER)
                            {
                                frame_started = true;
                                buffer_index = 0;
                                read_buffer[buffer_index++] = byte; // 存储帧头
                            }
                        }
                        else
                        {
                            // 正在接收一帧数据
                            read_buffer[buffer_index++] = byte;

                            if(buffer_index == 2)
                            {
                                // 第二个字节是帧长度
                                read_frame_length = byte;   // 帧长度包括帧头、帧尾和数据长度
                                if(read_frame_length > MAX_FRAME_SIZE)
                                {
                                    RCLCPP_ERROR(this->get_logger(), "Invalid frame length: %zu", read_frame_length);
                                    frame_started = false;  // 重置状态
                                }
                            }

                            // 检查是否接收到完整的帧
                            if(buffer_index == read_frame_length)
                            {
                                if(read_buffer[read_frame_length - 1] == FRAME_FOOTER)
                                {
                                    // 帧尾匹配，处理完整帧
                                    processReceivedData(read_buffer, read_frame_length);
                                }
                                else
                                {
                                    RCLCPP_ERROR(this->get_logger(), "Frame footer mismatch");
                                }
                                frame_started = false;  // 重置状态
                            }
                        }
                    }
                    else if(bytes_read == -1)
                    {
                        // RCLCPP_INFO(this->get_logger(), "Failed to read from serial port: %s", strerror(errno));
                    }
                }
                std::this_thread::sleep_for(std::chrono::milliseconds(10)); // 适当休眠，避免CPU占用过高
            }
        }

        // 处理接收到的数据
        void processReceivedData(const uint8_t* data, size_t length)
        {
            // 解析帧数据
            if(length < 3)
            {
                RCLCPP_ERROR(this->get_logger(), "Invalid frame length: %zu", length);
                return;
            }

            // 帧长度
            uint8_t frame_length = data[1];
            if(frame_length != length)
            {
                RCLCPP_ERROR(this->get_logger(), "Frame length mismatch: expected %zu, got %d", length, frame_length);
                return;
            }

            // 帧尾
            uint8_t frame_footer = data[length - 1];
            if(frame_footer != 0xFE)
            {
                RCLCPP_ERROR(this->get_logger(), "Invalid frame footer: 0x%02X", frame_footer);
                return;
            }

            // 提取数据部分(去掉帧头和帧尾)
            const uint8_t *payload = data + 2;  // 数据从第3字节开始
            size_t payload_length = length - 3; // 数据长度 = 总长度 - 帧头 -帧长度 - 帧尾

            // 处理有效数据
            std::string received_data(reinterpret_cast<const char *>(payload), payload_length);
            RCLCPP_INFO(this->get_logger(), "Received payload: %s", received_data.c_str());
        }

        // 发送数据的结构体
        struct SendData
        {
            double send_yaw;
            double send_pitch;
            double distance;
            char fire;
            char send_state;
        };

        // 回调接收参数
        rclcpp::Subscription<auto_aim_interfaces::msg::Armors>::SharedPtr armors_sub_;
        // 发送相关参数
        double send_yaw, send_pitch;
        double distance;
        int serial_fd, fd1, fd2, fd3;
        char fire, send_state; 
        // 卡尔曼相关参数
        std::mutex serial_mutex; // 锁
        std::mutex state_mutex_; // 新增状态锁
        double image_width, image_height, focal_length;
        std::unique_ptr<demo_cpp_tf::SimpleKalmanFilter> kf_;
        bool is_kf_initialized = false;
        // 开火条件相关参数
        const double FIRE_ANGLE_THRESHOLD = 0.1;     // 弧度（约2.8度）
        const double MIN_FIRE_DISTANCE = 0.1;         // 最小开火距离（米）
        const double MAX_FIRE_DISTANCE = 4.0;         // 最大开火距离（米）
        const int STABLE_FRAMES = 5;                  // 持续稳定帧数
        
        std::deque<std::pair<double, double>> angle_errors_; // 角度误差历史（yaw误差, pitch误差）
        rclcpp::Time last_aim_time_;                   // 最近一次对准时间
        bool is_aiming_stable_ = false;                // 是否持续稳定对准

        // 接收线程
        std::thread receive_thread_;

        // 发送线程
        std::thread send_thread_;

        // 发送数据队列
        std::queue<SendData> send_queue_;
        std::mutex send_mutex_; // 保护发送队列的锁
        // 定时器
        rclcpp::Time last_armor_time_;
        std::mutex time_mutex_;
        rclcpp::TimerBase::SharedPtr check_timer_;
};

int main(int argc, char** argv)
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<SerialNode>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}